%[T1Est, aEst,bEst, res,sd] = COSATIRT1Fitting(data, nlsS,C)

function [T1Est, aEst,bEst, res,sd] = COSATIRT1Fitting(data, nlsS,C)

data = data(nlsS.FailedMocoBit==0);
if ( length(data) ~= length(nlsS.tINVVec) )
    error('nlsS.N and data must be of equal length!')
end
% Make sure data is a column vector
data = data(:);
nlsS.dataPolar = nlsS.dataPolar(:);
nlsS.tINVVec = nlsS.tINVVec(:);
nlsS.tSATVec = nlsS.tSATVec (:);
%sort the data as inverison time

%% first, fit the last several images prepared by the SA+IR
dataPolar =  nlsS.dataPolar(:);
fitDat = data(dataPolar==0);
Tsat = nlsS.tSATVec (dataPolar==0);
Tinv =  nlsS.tINVVec(dataPolar==0);

[Tinv,tmOrderSL] = sort(Tinv);
Tsat = Tsat(tmOrderSL);
fitdat = fitDat(tmOrderSL);
 [aEst, bEst,T1Est,res]=invdatafit(Tsat,Tinv,fitdat);
 
 %generate polarity for images prepared by the second SAPPHIRE 
 Yi = aEst*(1-( 2- exp(-nlsS.tSATVec /T1Est)).*exp(-nlsS.tINVVec/T1Est))+bEst ;
datCPolar = Yi./abs(Yi );
data(dataPolar==0) = data(dataPolar==0).*datCPolar(dataPolar==0) ;


Tsat = nlsS.tSATVec ([1; find(dataPolar==1)]);
Tinv =  nlsS.tINVVec([1;  find(dataPolar==1)]);

[Tinv,tmOrderSL] = sort(Tinv);
Tsat = Tsat(tmOrderSL);
fitdat = fitDat(tmOrderSL);
 [aEst, bEst,T1Est,res]=invdatafit(Tsat,Tinv,fitdat);
%generate polarity for images prepared by the first SAPPHIRE
Yi = aEst*(1-( 2- exp(-nlsS.tSATVec /T1Est)).*exp(-nlsS.tINVVec/T1Est))+bEst ;
datCPolar = Yi./abs(Yi );
data(dataPolar==1) = data(dataPolar==1).*datCPolar(dataPolar==1) ;

%% fitting process
x0 = [aEst, T1Est, bEst];
x = fminsearch( ...
    @(x)sum( abs( data - ( x(1)*(1-(2-exp(-nlsS.tSATVec/x(2))).*exp(-nlsS.tINVVec/x(2)))+x(3))).^2 ), ...
    x0,optimset('display','off')); 


%get T1
aEst = x(1);
T1Est = x(2) ;
bEst = x(3);

x(1) = aEst ;
x(2) = T1Est;
% Compute the residual
modelValue = x(1)-x(1)*(2-exp(-nlsS.tSATVec/x(2))).*exp(-nlsS.tINVVec/x(2))+x(3);
res = 1/sqrt(length(data)) * norm(1 - modelValue./data);
%Computer the sdi
sd =0;
end
function [aEst, bEst,T1Est,res]=invdatafit(tsat,tinv,dat)

[Ind, tag] = find(tinv<1000);
Ind = [0;Ind];
aEstHat  = zeros(length(Ind),1);
bEstHat = zeros(length(Ind),1);
T1EstHat = zeros(length(Ind),1);
resHat = zeros(length(Ind),1);
for ind = 1:length( Ind )
    
    minInd =Ind(ind);
    dataTmp = dat.*[-ones(minInd,1); ones(length(dat) - minInd,1)];
    x0 = [ max(dat)  1400 max(dat) ];
    [aEst, bEst,T1Est]= subfitting(tsat,tinv,dataTmp,x0);
    aEstHat(ind) = aEst;
    bEstHat(ind) = bEst;
    T1EstHat(ind) = T1Est ;
    modelValue = aEstHat(ind)*(1-(2-exp( - tsat/T1EstHat(ind) )).*exp(-tinv/T1EstHat(ind)))+bEstHat(ind);
    resHat(ind) = 1/sqrt(length(dat)) * norm(1 - modelValue./dataTmp);
end
[v,ind] = min(resHat);

aEst = aEstHat(ind(1),1);
bEst = bEstHat(ind(1),1);
T1Est = T1EstHat(ind(1),1);
res = resHat(ind(1),1);
end
function [aEst, bEst,T1Est]=subfitting(tsat,tinv, data,x0 )
if( length(x0) == 3 )
    try
        x = fminsearch( ...
            @(x)sum(  abs( data- (  x(1)*(1-( 2- exp(-tsat/x(2))).*exp(-tinv/x(2))  )+x(3)) ).^2  ), ...
            x0,optimset('display','off'));
        
    catch
        disp('there is an error');
        x=x0 ;
    end
    aEst = x(1);
    bEst = x(3);
    T1Est = x(2);
else
    try
        x = fminsearch( ...
            @(x)sum( abs( data- (  x(1)-x(1)*( 2- exp(-tsat/x(2))).*exp(-tinv/x(2))  ) ).^2 ), ...
            x0,optimset('display','off'));
    catch
        disp('there is an error');
        x0
        x = x0;
    end
    aEst = x(1);
    bEst = 0;
    T1Est = x(2);
end
end
